using System;
using System.Collections.Generic;

namespace LaplaceMeshDeformer
{
    /// <summary>
    /// 拉普拉斯网格变形类
    /// 实现基于拉普拉斯坐标的网格变形算法
    /// </summary>
    public class LaplaceDeformer
    {
        private TriMesh mesh;
        private SparseMatrix laplacianMatrix; 
        private Dictionary<int, Vector3D> anchorVertices; // 锚点（顶点索引 -> 目标位置）
        private List<Vector3D> anchorPositions; // 锚点位置
        private List<int> anchorIndices; // 锚点顶点索引
        private List<Vector3D> deltaCoordinates; // 拉普拉斯坐标
        private bool useCotangentWeights; // 是否使用余切权重

        /// <summary>
        /// 获取锚点数量
        /// </summary>
        public int AnchorCount => anchorVertices.Count;

        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="mesh">要变形的网格</param>
        public LaplaceDeformer(TriMesh mesh)
        {
            this.mesh = mesh;
            this.anchorVertices = new Dictionary<int, Vector3D>();
            this.anchorPositions = new List<Vector3D>();
            this.anchorIndices = new List<int>();
            this.deltaCoordinates = new List<Vector3D>();
            this.useCotangentWeights = false;

            // 确保网格已经初始化连接关系
            TriMeshUtil.PreModifyGeometry(mesh);

            // 计算拉普拉斯矩阵和坐标
            ComputeLaplacianMatrix();
            ComputeDeltaCoordinates();
        }

        /// <summary>
        /// 添加锚点（控制点）
        /// </summary>
        /// <param name="vertexIndex">顶点索引</param>
        /// <param name="targetPosition">目标位置</param>
        /// <returns>操作是否成功</returns>
        public bool AddAnchor(int vertexIndex, Vector3D targetPosition)
        {
            if (vertexIndex < 0 || vertexIndex >= mesh.VertexCount)
            {
                return false;
            }

            // 如果锚点已存在，更新位置
            if (anchorVertices.ContainsKey(vertexIndex))
            {
                anchorVertices[vertexIndex] = targetPosition;
            }
            else
            {
                anchorVertices.Add(vertexIndex, targetPosition);
                anchorIndices.Add(vertexIndex);
                anchorPositions.Add(targetPosition);
            }

            return true;
        }

        /// <summary>
        /// 移除锚点
        /// </summary>
        /// <param name="vertexIndex">顶点索引</param>
        /// <returns>操作是否成功</returns>
        public bool RemoveAnchor(int vertexIndex)
        {
            if (!anchorVertices.ContainsKey(vertexIndex))
            {
                return false;
            }

            int index = anchorIndices.IndexOf(vertexIndex);
            if (index != -1)
            {
                anchorIndices.RemoveAt(index);
                anchorPositions.RemoveAt(index);
            }

            anchorVertices.Remove(vertexIndex);
            return true;
        }

        /// <summary>
        /// 清除所有锚点
        /// </summary>
        public void ClearAnchors()
        {
            anchorVertices.Clear();
            anchorIndices.Clear();
            anchorPositions.Clear();
        }

        /// <summary>
        /// 计算均匀权重拉普拉斯矩阵
        /// </summary>
        public void ComputeLaplacianMatrix()
        {
            int vertexCount = mesh.VertexCount;
            laplacianMatrix = new SparseMatrix(vertexCount, vertexCount);

            // 遍历每个顶点
            for (int i = 0; i < vertexCount; i++)
            {
                TriMesh.Vertex vertex = mesh.GetVertex(i);
                List<int> neighbors = GetVertexNeighbors(i);

                if (neighbors.Count > 0)
                {
                    // 如果使用均匀权重，给每个相邻顶点分配相同的权重
                    double weight = -1.0 / neighbors.Count;

                    // 设置对角线元素为1
                    laplacianMatrix.SetElement(i, i, 1.0);

                    // 设置邻居权重
                    foreach (int neighbor in neighbors)
                    {
                        laplacianMatrix.SetElement(i, neighbor, weight);
                    }
                }
                else
                {
                    // 如果没有邻居，则仅设置对角线元素为1
                    laplacianMatrix.SetElement(i, i, 1.0);
                }
            }
        }

        /// <summary>
        /// 计算余切权重拉普拉斯矩阵
        /// </summary>
        public void ComputeCotangentLaplacian()
        {
            int vertexCount = mesh.VertexCount;
            laplacianMatrix = new SparseMatrix(vertexCount, vertexCount);

            // 确保网格已经计算了面法线和顶点法线
            TriMeshUtil.PreModifyGeometry(mesh);

            // 初始化权重总和数组
            double[] weightSums = new double[vertexCount];
            for (int i = 0; i < vertexCount; i++)
            {
                weightSums[i] = 0.0;
            }

            // 遍历每个面，计算余切权重
            for (int faceIndex = 0; faceIndex < mesh.FaceCount; faceIndex++)
            {
                TriMesh.Face face = mesh.GetFace(faceIndex);
                int v0 = face.GetVertexIndex(0);
                int v1 = face.GetVertexIndex(1);
                int v2 = face.GetVertexIndex(2);

                Vector3D p0 = mesh.GetVertex(v0).Position;
                Vector3D p1 = mesh.GetVertex(v1).Position;
                Vector3D p2 = mesh.GetVertex(v2).Position;

                // 计算边的向量
                Vector3D e01 = p1 - p0;
                Vector3D e12 = p2 - p1;
                Vector3D e20 = p0 - p2;

                // 计算余切角
                double cot0 = CotanAngle(e20, e01);
                double cot1 = CotanAngle(e01, e12);
                double cot2 = CotanAngle(e12, e20);

                // 更新拉普拉斯矩阵和权重总和
                // 对于边 (v1, v2) 对应的是角 v0 处的余切值
                double w12 = cot0;
                double w20 = cot1;
                double w01 = cot2;

                // 累加权重到拉普拉斯矩阵
                AddCotangentWeight(v0, v1, w01, weightSums);
                AddCotangentWeight(v1, v2, w12, weightSums);
                AddCotangentWeight(v2, v0, w20, weightSums);
            }

            // 设置对角线元素
            for (int i = 0; i < vertexCount; i++)
            {
                laplacianMatrix.SetElement(i, i, 1.0);
            }

            // 更新拉普拉斯坐标
            ComputeDeltaCoordinates();
            useCotangentWeights = true;
        }

        /// <summary>
        /// 添加余切权重到拉普拉斯矩阵
        /// </summary>
        private void AddCotangentWeight(int i, int j, double weight, double[] weightSums)
        {
            if (weight < 0) weight = 0; // 处理非流形区域或退化三角形

            // 添加权重到矩阵元素
            double oldValue = laplacianMatrix.GetElement(i, j);
            laplacianMatrix.SetElement(i, j, oldValue - weight);

            // 更新权重总和
            weightSums[i] += weight;
        }

        /// <summary>
        /// 计算余切角
        /// </summary>
        private double CotanAngle(Vector3D a, Vector3D b)
        {
            // cot(angle) = (a·b)/(|a×b|)
            double dot = a.Dot(b);
            double cross = a.Cross(b).Length();

            if (Math.Abs(cross) < 1e-8)
                return 0; // 避免除零错误

            return dot / cross;
        }

        /// <summary>
        /// 计算拉普拉斯坐标（Delta坐标）
        /// </summary>
        public void ComputeDeltaCoordinates()
        {
            deltaCoordinates.Clear();

            int vertexCount = mesh.VertexCount;
            for (int i = 0; i < vertexCount; i++)
            {
                Vector3D delta = new Vector3D(0, 0, 0);
                TriMesh.Vertex vertex = mesh.GetVertex(i);
                Vector3D vertexPos = vertex.Position;

                List<int> neighbors = GetVertexNeighbors(i);
                if (neighbors.Count > 0)
                {
                    double weightSum = 0;

                    if (useCotangentWeights)
                    {
                        foreach (int j in neighbors)
                        {
                            double weight = -laplacianMatrix.GetElement(i, j);
                            Vector3D neighborPos = mesh.GetVertex(j).Position;
                            delta += (neighborPos - vertexPos) * weight;
                            weightSum += weight;
                        }

                        if (weightSum > 0)
                        {
                            delta /= weightSum;
                        }
                    }
                    else
                    {
                        // 均匀权重
                        foreach (int j in neighbors)
                        {
                            Vector3D neighborPos = mesh.GetVertex(j).Position;
                            delta += neighborPos;
                        }
                        delta = delta / neighbors.Count - vertexPos;
                    }
                }

                deltaCoordinates.Add(delta);
            }
        }

        /// <summary>
        /// 执行网格变形
        /// </summary>
        /// <returns>变形是否成功</returns>
        public bool Deform()
        {
            if (anchorVertices.Count == 0)
            {
                return false;
            }

            int vertexCount = mesh.VertexCount;
            LinearSystem system = new LinearSystem(vertexCount);

            // 设置系统矩阵（拉普拉斯算子）
            for (int i = 0; i < vertexCount; i++)
            {
                for (int j = 0; j < vertexCount; j++)
                {
                    double value = laplacianMatrix.GetElement(i, j);
                    if (value != 0)
                    {
                        system.SetMatrixElement(i, j, value);
                    }
                }
            }

            // 设置右侧向量 (b) 为拉普拉斯坐标
            for (int i = 0; i < vertexCount; i++)
            {
                Vector3D delta = deltaCoordinates[i];
                system.SetRightHandSide(i, 0, delta.X);
                system.SetRightHandSide(i, 1, delta.Y);
                system.SetRightHandSide(i, 2, delta.Z);
            }

            // 为锚点设置约束
            foreach (KeyValuePair<int, Vector3D> anchor in anchorVertices)
            {
                int index = anchor.Key;
                Vector3D targetPos = anchor.Value;

                // 清除该行的所有系数
                for (int j = 0; j < vertexCount; j++)
                {
                    system.SetMatrixElement(index, j, 0);
                }

                // 设置对角线元素为1
                system.SetMatrixElement(index, index, 1);

                // 设置右侧向量为目标位置
                system.SetRightHandSide(index, 0, targetPos.X);
                system.SetRightHandSide(index, 1, targetPos.Y);
                system.SetRightHandSide(index, 2, targetPos.Z);
            }

            // 求解线性系统
            bool solved = system.Solve();
            if (!solved)
            {
                return false;
            }

            // 更新网格顶点位置
            for (int i = 0; i < vertexCount; i++)
            {
                Vector3D newPosition = new Vector3D(
                    system.GetSolution(i, 0),
                    system.GetSolution(i, 1),
                    system.GetSolution(i, 2)
                );

                mesh.GetVertex(i).Position = newPosition;
            }

            // 更新网格法线
            TriMeshUtil.ComputeFaceNormals(mesh);
            TriMeshUtil.ComputeVertexNormals(mesh);

            return true;
        }

        /// <summary>
        /// 区域缩放变形
        /// </summary>
        /// <param name="centerVertexIndex">中心顶点索引</param>
        /// <param name="radius">影响半径</param>
        /// <param name="scaleFactor">缩放因子</param>
        /// <returns>变形是否成功</returns>
        public bool ScaleDeform(int centerVertexIndex, double radius, double scaleFactor)
        {
            if (centerVertexIndex < 0 || centerVertexIndex >= mesh.VertexCount)
            {
                return false;
            }

            // 清除现有锚点
            ClearAnchors();

            Vector3D centerPos = mesh.GetVertex(centerVertexIndex).Position;
            double radiusSq = radius * radius;

            // 为受影响区域内的顶点设置锚点，缩放距离中心的位置
            for (int i = 0; i < mesh.VertexCount; i++)
            {
                Vector3D pos = mesh.GetVertex(i).Position;
                Vector3D offset = pos - centerPos;
                double distSq = offset.LengthSquared();

                if (distSq <= radiusSq)
                {
                    // 计算基于距离的缩放因子（线性衰减）
                    double distance = Math.Sqrt(distSq);
                    double factor = scaleFactor;
                    if (distance > 0)
                    {
                        // 距离越远，影响越小
                        factor = 1.0 + (scaleFactor - 1.0) * (1.0 - distance / radius);
                    }

                    Vector3D newPos = centerPos + offset * factor;
                    AddAnchor(i, newPos);
                }
            }

            // 执行变形
            return Deform();
        }

        /// <summary>
        /// 平滑处理
        /// </summary>
        /// <param name="iterations">迭代次数</param>
        /// <param name="lambda">平滑强度(0-1)</param>
        /// <returns>平滑是否成功</returns>
        public bool Smooth(int iterations, double lambda)
        {
            if (iterations <= 0 || lambda < 0 || lambda > 1)
            {
                return false;
            }

            int vertexCount = mesh.VertexCount;
            Vector3D[] newPositions = new Vector3D[vertexCount];
            Vector3D[] currentPositions = new Vector3D[vertexCount];

            // 备份当前顶点位置
            for (int i = 0; i < vertexCount; i++)
            {
                currentPositions[i] = mesh.GetVertex(i).Position;
            }

            // 多次迭代平滑
            for (int iter = 0; iter < iterations; iter++)
            {
                // 计算新位置
                for (int i = 0; i < vertexCount; i++)
                {
                    List<int> neighbors = GetVertexNeighbors(i);
                    if (neighbors.Count > 0)
                    {
                        Vector3D sum = new Vector3D(0, 0, 0);
                        foreach (int j in neighbors)
                        {
                            sum += currentPositions[j];
                        }

                        Vector3D average = sum / neighbors.Count;
                        newPositions[i] = currentPositions[i] * (1 - lambda) + average * lambda;
                    }
                    else
                    {
                        newPositions[i] = currentPositions[i];
                    }
                }

                // 更新位置
                for (int i = 0; i < vertexCount; i++)
                {
                    currentPositions[i] = newPositions[i];
                }
            }

            // 应用平滑后的位置
            for (int i = 0; i < vertexCount; i++)
            {
                mesh.GetVertex(i).Position = currentPositions[i];
            }

            // 更新网格法线
            TriMeshUtil.ComputeFaceNormals(mesh);
            TriMeshUtil.ComputeVertexNormals(mesh);

            // 更新拉普拉斯坐标
            ComputeDeltaCoordinates();

            return true;
        }

        /// <summary>
        /// 获取顶点的邻居顶点索引
        /// </summary>
        /// <param name="vertexIndex">顶点索引</param>
        /// <returns>邻居顶点索引列表</returns>
        private List<int> GetVertexNeighbors(int vertexIndex)
        {
            List<int> neighbors = new List<int>();
            TriMesh.Vertex vertex = mesh.GetVertex(vertexIndex);

            // 如果顶点没有半边，说明网格未建立连接关系
            if (vertex.Halfedge == null)
            {
                // 从面信息中查找邻居
                for (int f = 0; f < mesh.FaceCount; f++)
                {
                    TriMesh.Face face = mesh.GetFace(f);
                    bool containsVertex = false;
                    
                    for (int j = 0; j < 3; j++)
                    {
                        if (face.GetVertexIndex(j) == vertexIndex)
                        {
                            containsVertex = true;
                            break;
                        }
                    }

                    if (containsVertex)
                    {
                        for (int j = 0; j < 3; j++)
                        {
                            int neighborIndex = face.GetVertexIndex(j);
                            if (neighborIndex != vertexIndex && !neighbors.Contains(neighborIndex))
                            {
                                neighbors.Add(neighborIndex);
                            }
                        }
                    }
                }
            }
            else
            {
                // 遍历顶点的所有相邻边，获取相邻顶点
                TriMesh.Halfedge start = vertex.Halfedge;
                TriMesh.Halfedge current = start;

                do
                {
                    int targetIndex = current.Target.Index;
                    if (!neighbors.Contains(targetIndex))
                    {
                        neighbors.Add(targetIndex);
                    }

                    if (current.Twin == null)
                        break;
                        
                    current = current.Twin.Next;
                    
                    if (current == null)
                        break;
                        
                } while (current != start && current != null);
            }

            return neighbors;
        }
    }
} 